#include "scheduler_task.h"
volatile int time_tick_1ms=0;
void Loop_100Hz()
{
	WorkStateFSM();
	CAN_Watch();
	mpu_get_data();
	imu_ahrs_update();
	imu_attitude_update();
	Gimbal_Control();
	ALL_Motor_Out();
}

void Loop_25Hz()
{
	Chassis_Control();
	Shoot_Control();
	Ammunition_supply_Control();
}
void Loop_10Hz()
{
	Volt_Sendware();
}
static scheduler_t SCHED_TASK[]={
	{Loop_100Hz,100,0,0},
	{Loop_25Hz ,25 ,0,0},
	{Loop_10Hz ,10 ,0,0}};
#define TASK_NUM (sizeof(SCHED_TASK)/sizeof(scheduler_t))
void Scheduler_Setup()
{
	uint8_t index;
	for(index=0;index<TASK_NUM;index++)
	{
		SCHED_TASK[index].interval_tick = 1000/SCHED_TASK[index].rate_hz;
		if(SCHED_TASK[index].interval_tick<1)
		{
			SCHED_TASK[index].interval_tick=1;
		}
	}
}	

void Scheduler_Run()
{
	uint8_t index;
	for(index=0;index<TASK_NUM;index++)
	{
		uint32_t tnow=HAL_GetTick();
		if(tnow-SCHED_TASK[index].last_rate>=SCHED_TASK[index].interval_tick)
		{
			SCHED_TASK[index].task_fuc();
			SCHED_TASK[index].last_rate = tnow;
		}
	}
}

